﻿#include "MathUtil.h"

Eigen::VectorXd polynomialInterpolation(const std::vector<Point>& points)
{
	int n = points.size();
	Eigen::MatrixXd A(n, n);
	Eigen::VectorXd y(n);
    
	// 构建Vandermonde矩阵
	for (int i = 0; i < n; ++i)
	{
		double x = points[i].first;
		y(i) = points[i].second;
		A(i, 0) = 1.0;
		for (int j = 1; j < n; ++j)
		{
			A(i, j) = A(i, j-1) * x;
		}
	}
    
	// 求解线性方程组
	return A.colPivHouseholderQr().solve(y);
}

double evaluatePolynomial(const Eigen::VectorXd& coeffs, double x)
{
	double result = 0.0;
	double xPower = 1.0;  // x^0
	for (int i = 0; i < coeffs.size(); ++i)
	{
		result += coeffs(i) * xPower;
		xPower *= x;
	}
	return result;
}

double gaussianBasis(double x, double center, double sigma)
{
	const double dx = x - center;
	return std::exp(-dx * dx / (2 * sigma * sigma));
}

Eigen::VectorXd gaussBasisInterpolation(const std::vector<Point>& points, double sigma)
{
	const int m = points.size();
	if (m == 0)
	{
		return Eigen::VectorXd();
	}

	// 构建 m×m 系数矩阵 A 和 m×1 观测向量 y
	Eigen::MatrixXd A(m, m);
	Eigen::VectorXd y(m);

	for (int j = 0; j < m; ++j)
	{
		const double x_j = points[j].first;  // 第j个插值节点的x坐标
		y(j) = points[j].second;             // 第j个插值节点的y坐标（方程右侧）

		A(j, 0) = 1.0;  // 第j行第0列：b0的系数恒为1

		// 第j行第i列（i≥1）：基函数g_i(x_j)的取值
		// 基函数中心 x_{i-1} = 第i-1个插值节点的x坐标
		for (int i = 1; i < m; ++i)
		{
			const double center = points[i-1].first;  // 基函数中心=插值点x坐标
			A(j, i) = gaussianBasis(x_j, center, sigma);
		}
	}

	// 用列主元Householder QR分解求解方程组 A*b = y（适定问题，唯一解）
	Eigen::VectorXd b = A.colPivHouseholderQr().solve(y);

	return b;
}

double evaluateGaussBasis(const Eigen::VectorXd& coefficients, const std::vector<Point>& points, double x, double sigma)
{
	const int m = points.size();
	if (m != coefficients.size())
	{
		return 0.0;
	}

	double result = coefficients(0);  // b0
	for (int i = 1; i < m; ++i)
	{
		const double center = points[i-1].first;  // 基函数中心=第i-1个插值点x坐标
		result += coefficients(i) * gaussianBasis(x, center, sigma);
	}
	return result;
}

double computeGlobalAvgSpacing(const std::vector<Point>& points)
{
	const int m = points.size();
	if (m <= 1) return 1.0; // 节点数≤1时，用默认σ=1

	// 先对节点按x坐标升序排序（确保计算相邻间距正确）
	std::vector<Point> sortedPoints = points;
	std::sort(sortedPoints.begin(), sortedPoints.end(),
		[](const Point& a, const Point& b) { return a.first < b.first; });

	// 计算相邻节点的间距和
	double totalSpacing = 0.0;
	for (int i = 1; i < m; ++i) {
		totalSpacing += sortedPoints[i].first - sortedPoints[i - 1].first;
	}

	// 平均间距 = 总间距 / (节点数-1)
	return totalSpacing / (m - 1);
}

double adaptiveGlobalSigma(const std::vector<Point>& points, double k)
{
	double d_avg = computeGlobalAvgSpacing(points);
	return std::max(k * d_avg, 1e-3); // 避免sigma过小（小于1e-3会导致基函数几乎为0）
}

Eigen::VectorXd polynomialLeastSquaresFitting(const std::vector<Point>& points, int max_degree)
{
	const int n = points.size(); // 数据点个数
	const int param_count = max_degree + 1; // 参数个数：a0~am（共m+1个）

	// 合法性检查
	if (n == 0) 
	{
		return Eigen::VectorXd();
	}
	if (max_degree < 0 || max_degree >= n) 
	{
		return Eigen::VectorXd();
	}

	// 1. 构建设计矩阵A（n×(m+1)）和观测向量y（n×1）
	Eigen::MatrixXd A(n, param_count);
	Eigen::VectorXd y(n);

	for (int i = 0; i < n; ++i) 
	{
		const double x_i = points[i].first;
		y(i) = points[i].second; // 观测向量：数据点的y值

		// 设计矩阵第i行：[1, x_i, x_i^2, ..., x_i^m]（递推计算避免重复幂运算）
		A(i, 0) = 1.0; // x_i^0 = 1
		for (int j = 1; j <= max_degree; ++j) 
		{
			A(i, j) = A(i, j - 1) * x_i; // x_i^j = x_i^(j-1) * x_i
		}
	}

	// 2. 用SVD分解求解最小二乘解（数值稳定性最优）
	// JacobiSVD：Eigen中用于最小二乘的稳定求解器，ComputeThinU/V优化计算效率
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(
		A, Eigen::ComputeThinU | Eigen::ComputeThinV
	);
	Eigen::VectorXd a = svd.solve(y); // 自动计算最小二乘解

	return a;
}

Eigen::VectorXd ridgeRegressionFitting(const std::vector<Point>& points, int max_degree, double lambda)
{
	const int n = points.size();
	const int param_count = max_degree + 1; // 参数个数：a0~am（m+1个）

	// 合法性检查
	if (n == 0) {
		return Eigen::VectorXd();
	}
	if (max_degree < 0 || max_degree >= n) 
	{
		return Eigen::VectorXd();
	}
	if (lambda < 0.0) 
	{
		return Eigen::VectorXd();
	}

	// 1. 构建设计矩阵A（n×(m+1)）和观测向量y（n×1）
	Eigen::MatrixXd A(n, param_count);
	Eigen::VectorXd y(n);
	for (int i = 0; i < n; ++i) 
	{
		const double x_i = points[i].first;
		y(i) = points[i].second;

		// 递推计算A的第i行：[1, x_i, x_i^2, ..., x_i^m]
		A(i, 0) = 1.0;
		for (int j = 1; j <= max_degree; ++j) 
		{
			A(i, j) = A(i, j - 1) * x_i;
		}
	}

	// 2. 构造岭回归正规方程组：(A^T A + λI) * a = A^T y
	Eigen::MatrixXd ATA = A.transpose() * A; // A^T A：(m+1)×(m+1)
	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(param_count, param_count); // 单位矩阵
	Eigen::MatrixXd reg_ATA = ATA + lambda * I; // 加入正则项后的系数矩阵
	Eigen::VectorXd ATy = A.transpose() * y; // 右侧向量：A^T y

	// 3. 求解正规方程组（Cholesky分解，适用于正定矩阵，高效稳定）
	Eigen::VectorXd a = reg_ATA.llt().solve(ATy); // LLT是Cholesky分解的接口

	return a;
}

std::vector<Point> generateFitPoints(std::function<double(double)> fitFunction, const std::vector<Point>& originalPoints, int n)
{
	if (n <= 0) return std::vector<Point>();
	if (originalPoints.empty()) return std::vector<Point>();
    
	// 1. 确定x范围（原始点的x最小值和最大值，扩展10%边界使曲线更完整）
	double xMin = originalPoints[0].first;
	double xMax = originalPoints[0].first;
	for (const auto& p : originalPoints)
	{
		xMin = std::min(xMin, p.first);
		xMax = std::max(xMax, p.first);
	}
	// 扩展边界（避免曲线在端点突然截断）
	double margin = 0.1 * (xMax - xMin);
	xMin -= margin;
	xMax += margin;
	// 所有点x相同的特殊情况
	if (xMin == xMax)
	{
		xMin -= 0.5;
		xMax += 0.5;
	}
    
	// 2. 生成n个均匀分布的x值
	std::vector<Point> fitPoints;
	fitPoints.reserve(n);
	double step = (xMax - xMin) / (n - 1); // 步长
	for (int i = 0; i < n; ++i)
	{
		double x = xMin + i * step;
		double y = fitFunction(x); // 用拟合函数计算y值
		fitPoints.emplace_back(x, y);
	}
	return fitPoints;
}

std::vector<double> generateEquidistantT(const std::vector<Point>& points)
{
	const int n = points.size();
	if (n <= 1) return { 0.0 };
	std::vector<double> t(n);
	for (int i = 0; i < n; ++i) 
	{
		t[i] = static_cast<double>(i) / (n - 1); // 归一化到 [0,1]
	}
	return t;
}

std::vector<double> generateChordLengthT(const std::vector<Point>& points)
{
	const int n = points.size();
	if (n <= 1) return { 0.0 };
	std::vector<double> t(n, 0.0);
	double total_length = 0.0;

	// 计算累积弦长
	for (int i = 1; i < n; ++i) 
	{
		const double dx = points[i].first - points[i - 1].first;
		const double dy = points[i].second - points[i - 1].second;
		const double dist = std::sqrt(dx * dx + dy * dy);
		t[i] = t[i - 1] + dist;
		total_length += dist;
	}

	// 归一化到 [0,1]
	if (total_length > 1e-10) 
	{
		for (double& ti : t) ti /= total_length;
	}
	return t;
}

std::vector<double> generateCentripetalT(const std::vector<Point>& points, double alpha)
{
	const int n = points.size();
	if (n <= 1) return { 0.0 };
	if (alpha <= 0.0 || alpha > 1.0) 
	{
		return generateEquidistantT(points);
	}

	std::vector<double> t(n, 0.0);
	double total_t = 0.0;

	// 递推计算t_i = t_{i-1} + (Δs_i)^alpha
	for (int i = 1; i < n; ++i) 
	{
		const double dx = points[i].first - points[i - 1].first;
		const double dy = points[i].second - points[i - 1].second;
		const double delta_s = std::sqrt(dx * dx + dy * dy);
		const double delta_t = std::pow(delta_s, alpha);
		t[i] = t[i - 1] + delta_t;
		total_t = t[i];
	}

	// 归一化到[0,1]（避免总长度为0的极端情况）
	if (total_t > 1e-10) 
	{
		for (double& ti : t) ti /= total_t;
	}
	else 
	{
		// 所有点重合，返回等距t
		for (int i = 0; i < n; ++i) t[i] = static_cast<double>(i) / (n - 1);
	}

	return t;
}

std::vector<double> generateFoleyT(const std::vector<Point>& points)
{
	const int n = points.size();
	if (n <= 1) return { 0.0 };
	if (n == 2) 
	{
		// 两点仅一条线段，返回等距t（[0,1]）
		return { 0.0, 1.0 };
	}

	std::vector<double> t(n, 0.0);
	const int m = n - 1; // 线段数量

	// 步骤1：计算所有相邻线段的长度Δs_i（i=1~m）
	std::vector<double> delta_s(m, 0.0);
	for (int i = 0; i < m; ++i) 
	{
		const double dx = points[i + 1].first - points[i].first;
		const double dy = points[i + 1].second - points[i].second;
		delta_s[i] = std::sqrt(dx * dx + dy * dy);
	}

	// 步骤2：计算每个点的张力参数s_i（i=0~n-1）
	std::vector<double> s(n, 0.0);
	const double eps = 1e-6; // 避免除零的阈值

	// 边界点：首点（i=0）
	s[0] = delta_s[0];
	// 边界点：末点（i=n-1）
	s[n - 1] = delta_s[m - 1];

	// 内部点（i=1~n-2）
	for (int i = 1; i <= n - 2; ++i) 
	{
		// 向量v_i^- = P_i - P_{i-1}，v_i^+ = P_{i+1} - P_i
		const double vx_minus = points[i].first - points[i - 1].first;
		const double vy_minus = points[i].second - points[i - 1].second;
		const double vx_plus = points[i + 1].first - points[i].first;
		const double vy_plus = points[i + 1].second - points[i].second;

		// 计算向量长度（已提前存储delta_s[i-1] = ||v_i^-||，delta_s[i] = ||v_i^+||）
		const double len_minus = delta_s[i - 1];
		const double len_plus = delta_s[i];
		if (len_minus < eps || len_plus < eps) 
		{
			// 相邻点重合，张力参数取默认值
			s[i] = 1.0;
			continue;
		}

		// 点积求cosθ_i
		const double dot = vx_minus * vx_plus + vy_minus * vy_plus;
		double cos_theta = dot / (len_minus * len_plus);
		// 钳位cosθ_i到[-1,1]（浮点误差导致溢出）
		cos_theta = std::clamp(cos_theta, -1.0 + eps, 1.0 - eps);

		// 计算sin(θ_i/2)：利用三角恒等式sin(θ/2) = sqrt((1 - cosθ)/2)
		const double sin_half_theta = std::sqrt((1.0 - cos_theta) / 2.0);
		if (sin_half_theta < eps) 
		{
			// 三点共线（θ≈0），张力参数取相邻线段长度平均值
			s[i] = (len_minus + len_plus) / 2.0;
		}
		else 
		{
			// 标准张力参数公式
			s[i] = (len_minus + len_plus) / (2.0 * sin_half_theta);
		}
	}

	// 步骤3：递推计算t_i = t_{i-1} + Δs_i / s_{i-1}
	double total_t = 0.0;
	for (int i = 1; i < n; ++i) 
	{
		if (s[i - 1] < eps) 
		{
			// 张力参数过小，避免除零，取Δs_i作为增量
			t[i] = t[i - 1] + delta_s[i - 1];
		}
		else 
		{
			t[i] = t[i - 1] + delta_s[i - 1] / s[i - 1];
		}
		total_t = t[i];
	}

	// 步骤4：归一化到[0,1]
	if (total_t > eps) 
	{
		for (double& ti : t) ti /= total_t;
	}
	else 
	{
		// 所有点重合，返回等距t
		for (int i = 0; i < n; ++i) t[i] = static_cast<double>(i) / (n - 1);
	}

	return t;
}

std::pair<Eigen::VectorXd, Eigen::VectorXd> fitParametricLeastSquares(const std::vector<Point>& points, int max_degree, const std::vector<double>& t)
{
	const int n = points.size();
	const int param_count = max_degree + 1; // 每个分量的参数个数（m+1）

	// 合法性检查
	if (n <= 1 || max_degree < 0 || max_degree >= n - 1) return { Eigen::VectorXd(), Eigen::VectorXd() };

	// 1. 构建设计矩阵 A（n × (m+1)，x(t) 和 y(t) 共享 A）
	Eigen::MatrixXd A(n, param_count);
	for (int i = 0; i < n; ++i) 
	{
		double ti = t[i];
		A(i, 0) = 1.0; // t^0 = 1
		// 递推计算 t^1, t^2, ..., t^m
		for (int j = 1; j <= max_degree; ++j) 
		{
			A(i, j) = A(i, j - 1) * ti;
		}
	}

	// 2. 分离 x 和 y 观测向量
	Eigen::VectorXd x_vec(n), y_vec(n);
	for (int i = 0; i < n; ++i) 
	{
		x_vec(i) = points[i].first;
		y_vec(i) = points[i].second;
	}

	// 3. SVD 分解求解最小二乘解（数值稳定）
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::VectorXd a = svd.solve(x_vec); // x(t) 的系数
	Eigen::VectorXd b = svd.solve(y_vec); // y(t) 的系数

	return { a, b };
}

Point evaluateFittedCurve(double t_eval, const std::pair<Eigen::VectorXd, Eigen::VectorXd>& coeffs)
{
	const int m = coeffs.first.size() - 1; // 最高次数 m
	if (coeffs.first.size() != coeffs.second.size()) 
	{
		return { 0.0, 0.0 };
	}

	// 计算 x(t_eval) = a0 + a1*t + ... + am*t^m
	double x_eval = 0.0, y_eval = 0.0;
	double t_power = 1.0; // 递推存储 t^0, t^1, ..., t^m
	for (int j = 0; j <= m; ++j) 
	{
		x_eval += coeffs.first(j) * t_power;
		y_eval += coeffs.second(j) * t_power;
		t_power *= t_eval;
	}
	return { x_eval, y_eval };
}

